/***************************************************************************//**
 * @file CGI/CGI_Manager.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * This file is part of MScSim. You can redistribute and modify it under
 * the terms of GNU General Public License as published by the Free Software
 * Foundation; either version 3 of the License, or (at your option) any later
 * version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.
 * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * Further information about the GNU General Public License can also be found
 * on the world wide web at http://www.gnu.org.
 ******************************************************************************/
#ifndef CGI_MANAGER_CPP
#define CGI_MANAGER_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#ifndef _USE_MATH_DEFINES
#   define _USE_MATH_DEFINES
#endif

////////////////////////////////////////////////////////////////////////////////

#include <cmath>
#include <iostream>
#include <memory.h>

#include <CGI/CGI_Manager.h>
#include <CGI/VisitorFindNode.h>

////////////////////////////////////////////////////////////////////////////////

CGI_Manager::CGI_Manager() :
    m_data ( new CGI_Data() )
{
    m_sceneRoot = new SceneRoot( m_data );

    m_camManipulatorInternal = new CamManipulatorInternal();
    m_camManipulatorExternal = new CamManipulatorExternal();

    m_camManipulatorType = InternalCamManipulator;
}

////////////////////////////////////////////////////////////////////////////////

CGI_Manager::~CGI_Manager()
{
    if ( m_sceneRoot ) delete m_sceneRoot; m_sceneRoot = 0;

    ////////////////////////////////////////
    if ( m_data ) delete m_data; m_data = 0;
    ////////////////////////////////////////
}

////////////////////////////////////////////////////////////////////////////////

void CGI_Manager::update( double /*timeStep*/, const CGI_DataInp &dataInp, CGI_DataOut &dataOut )
{
    m_data->dataInp = dataInp;

    if ( m_camManipulatorType == InternalCamManipulator )
    {
        osg::Quat  q1_temp;
        osg::Quat  q2_temp;
        osg::Vec3d l1_temp;

        q1_temp.makeRotate( -M_PI / 2.0, osg::Vec3d( 1.0, 0.0, 0.0 ),
                                    0.0, osg::Vec3d( 0.0, 1.0, 0.0 ),
                             M_PI / 2.0, osg::Vec3d( 0.0, 0.0, 1.0 ) );

        q2_temp.makeRotate( dataInp.aircraft.wgs_phi, osg::Vec3d( 1.0, 0.0, 0.0 ),
                            dataInp.aircraft.wgs_tht, osg::Vec3d( 0.0, 1.0, 0.0 ),
                            dataInp.aircraft.wgs_psi, osg::Vec3d( 0.0, 0.0, 1.0 ) );

        l1_temp = osg::Vec3d( dataInp.aircraft.wgs_x,
                              dataInp.aircraft.wgs_y,
                              dataInp.aircraft.wgs_z );

        osg::Matrixd matrix( osg::Matrixd::rotate( q1_temp )
                           * osg::Matrixd::rotate( q2_temp )
                           * osg::Matrixd::translate( l1_temp ) );

        m_camManipulatorInternal->setByMatrix( matrix );
    }

    switch ( m_camManipulatorType )
    {
    case InternalCamManipulator:
        m_data->cameraData.attitude = m_camManipulatorInternal->getMatrix().getRotate();
        m_data->cameraData.position = m_camManipulatorInternal->getMatrix().getTrans();
        break;

    case ExternalCamManipulator:
        m_data->cameraData.attitude = m_camManipulatorExternal->getMatrix().getRotate();
        m_data->cameraData.position = m_camManipulatorExternal->getMatrix().getTrans();
        break;
    }

    osg::ref_ptr<osg::EllipsoidModel> em = new osg::EllipsoidModel();

    em->convertXYZToLatLongHeight( m_data->cameraData.position.x(),
                                   m_data->cameraData.position.y(),
                                   m_data->cameraData.position.z(),
                                   m_data->cameraData.latitude,
                                   m_data->cameraData.longitude,
                                   m_data->cameraData.altitude );

    //////////////////////
    m_sceneRoot->update();
    //////////////////////

    dataOut = m_data->dataOut;
}

////////////////////////////////////////////////////////////////////////////////

void CGI_Manager::setTrackNode( osgGA::NodeTrackerManipulator *nodeTrackerManipulator )
{
    VisitorFindNode findNode( "AircraftModelGroup" );

    m_sceneRoot->getSceneRoot()->accept( findNode );

    osg::ref_ptr<osg::Node> aircraftModelsGroup = dynamic_cast <osg::Node *> ( findNode.getFirstNode().get() );

    if ( aircraftModelsGroup.valid() )
    {
        nodeTrackerManipulator->setTrackNode( aircraftModelsGroup.get() );
    }
}
